#include "UdpSocketClient.h"

namespace lslidar
{
  int UdpClient::init(uint16_t port)
  {
    udp_socket_ = socket(PF_INET, SOCK_DGRAM, 0);
    if (udp_socket_ == -1)
    {
      perror("socket error");
      return -1;
    }
    int socketoptvalue = 1;
    setsockopt(udp_socket_, SOL_SOCKET, SO_REUSEADDR,
               (const char *)&socketoptvalue, sizeof(int));
    struct timeval tv;
    tv.tv_sec = 0;
    tv.tv_usec = 200000; // 200 ms
    setsockopt(udp_socket_, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv,
               sizeof(struct timeval));

    sockaddr_in myAddress;                    // my address information
    memset(&myAddress, 0, sizeof(myAddress)); // initialize to zeros
    myAddress.sin_family = AF_INET;           // host byte order
    myAddress.sin_port = htons(port);         // port in network byte order
    myAddress.sin_addr.s_addr = INADDR_ANY;   // automatically fill in my IP
    if (bind(udp_socket_, reinterpret_cast<sockaddr *>(&myAddress),
             sizeof(sockaddr)) == -1)
    {
      perror("bind");
      return -1;
    }

    if (fcntl(udp_socket_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
    {
      perror("non-block");
      return -1;
    }

    return 0;
  }

  ssize_t UdpClient::readData(uint8_t *buf, ssize_t len)
  {
    struct sockaddr_in src;
    socklen_t src_len = sizeof(src);
    memset(&src, 0, sizeof(src));

    ssize_t nbytes = -1;
    nbytes = recvfrom(udp_socket_, buf, len, 0, (sockaddr *)(&src), &src_len);
    if (nbytes > 0)
    {
      return nbytes;
    }
    else
    {
      return -1;
    }
  }

} // namespace lslidar
